#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include <cmath>
#include <vector>


// 回到中间位置
bool returnToMiddle(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    move_group.setNamedTarget("middle");
    moveit::planning_interface::MoveGroupInterface::Plan home_plan;
    bool success = (move_group.plan(home_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success) {
        move_group.execute(home_plan);
        RCLCPP_INFO(logger, "Returned to middle position.");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to return to middle position.");
        return false;
    }
}

// 主函数
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("zc4_moveit_control");

    // 创建MoveGroupInterface对象，指定规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(node, "zc4");

    // 调用返回中间位置的函数
    rclcpp::Logger logger = node->get_logger();
    bool success = returnToMiddle(move_group, logger);

    if (success) {
        RCLCPP_INFO(logger, "Successfully returned to the middle position.");
    } else {
        RCLCPP_ERROR(logger, "Failed to return to the middle position.");
    }

    rclcpp::shutdown();
    return 0;
}
